Solving velocity deviation in Pilz #3158
Conversation
… velocityprofile_rect.
setting eqradius = 0.0 and replacing the cartesianTrapVelocityProfile() function with cartesianRectVelocityProfile for generating the velocity profile of the KDL path
|
Please target the |
Thanks @vivekcdavid! As this comment says, all new changes should go through to the I'll still leave a few comments for now in case you want to go at those too. |
| @@ -136,9 +136,17 @@ class TrajectoryGenerator | |||
| * The trap profile returns uses the longer distance of translational and | |||
| * rotational motion. | |||
| */ | |||
| std::unique_ptr<KDL::VelocityProfile> cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, | |||
| std::unique_ptr<KDL::VelocityProfile> cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, | |||
There was a problem hiding this comment.
this indentation may break the formatting -- accidental change?
| @@ -271,7 +279,7 @@ class TrajectoryGenerator | |||
| protected: | |||
| const moveit::core::RobotModelConstPtr robot_model_; | |||
| const pilz_industrial_motion_planner::LimitsContainer planner_limits_; | |||
| static constexpr double MIN_SCALING_FACTOR{ 0.0001 }; | |||
| static constexpr double MIN_SCALING_FACTOR{ 0.0000001 }; | |||
There was a problem hiding this comment.
This may end up changing default behavior -- but as a question to you, did you find this made a difference in other cases? Maybe this should also be a parameter that users can configure at runtime, whose default remains the former 1e-4?
Also minor nitpick, with all the zeros consider using scientific notation like 1e-7.
| //std::unique_ptr<KDL::VelocityProfile_Rectangular> vp_trans(new KDL::VelocityProfile_Rectangular()); | ||
| std::unique_ptr<KDL::VelocityProfile> vp_trans = std::make_unique<KDL::VelocityProfile_Rectangular>( | ||
| max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity()); | ||
| //std::unique_ptr<KDL::VelocityProfile> vp_trans = std::make_unique<KDL::VelocityProfile_Trap>( | ||
| // max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(), | ||
| // max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration()); | ||
|
|
||
| if (path->PathLength() > std::numeric_limits<double>::epsilon()) // avoid division by zero | ||
| { | ||
| //vp_trans->SetProfile(0, path->PathLength()); |
There was a problem hiding this comment.
make sure all these commented out lines are removed
| @@ -154,10 +154,12 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s | |||
| // create Cartesian path for lin | |||
| std::unique_ptr<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); | |||
|
|
|||
| // create velocity profile | |||
| // create velocity profile using trap profile | |||
| //std::unique_ptr<KDL::VelocityProfile> vp( | ||
| // cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); | ||
| // create velocity profile using rect profile |
| std::unique_ptr<KDL::VelocityProfile> vp( | ||
| cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); | ||
|
|
||
| cartesianRectVelocityProfile(req.max_velocity_scaling_factor, path)); |
There was a problem hiding this comment.
This cannot be just changed, it has to be a runtime parameter configurable by the user, whose default still retains trapezoidal for compatibility.
| //double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / | ||
| // planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); | ||
| double eqradius = 0.0; |
There was a problem hiding this comment.
same here -- probably whatever parameter is used to switch from trapezoidal to rectangle can also zero out the eq_radius.
The solution for solving the deviation as described in #2909.
The issue is that the Pilz LIN planner deviates from target linear speed when rotation is involved in a move. The issue seems to arise from two factors within Pilz, both related to KDL::Path_line.
While generating the path using KDL::Path_line a parameter called eqradius is used. The use of Eqradius prevents very fast angular motions but as a side effect the motion deviated from the specified linear speed. *
solution* : set eqradius to zero if the motion should only consider the specified linear TCP speed
Even after setting eqradius to zero we observed a small deviation in the time take for motion, this was pin-pointed to the use of trapezoidal velocity profile.
solution : By using a rectangular velocity profile this issue seems to be solved.
It would be nice if these changes can be used if the use-case requires that the time for motion has to be accurate. Now I have permanently changed the code but maybe we could force Pilz to use rectangular profile when acceleration limit is not specified as suggested by @sea-bass ?